/* a[1~n][1~n]: co-efficient matrix.
   b[1~n] : RHS vector.
   Solution is returned via b[1~n] and the co-efficient matrix is distroyed in the course of computation.
 */

# include <stdio.h>
# include <stdlib.h>
# include <math.h>

static void lubksb(double **a, unsigned short n, const int *indx, double *b);
static void ludcmp(double **a, unsigned short n, int *indx, double *d);
static void improve_sol(double **a0, double **alud, unsigned short n, const int *indx, const double *b0, double *x);

extern void solve_matrix(double **a, unsigned short n, double *b)
    {
     int *indx, i, k;
     double d, **a0, *b0;

     indx = (int *)malloc(sizeof(int)*(n+1));

     b0 = (double *)malloc(sizeof(double)*(n+1));

     a0 = (double **)malloc(sizeof(double *)*(n+1));
     for(i = 1; i <= n; i++) a0[i] = (double *)malloc(sizeof(double)*(n+1));

  /* Saving the matrix "a" into "a0" and "b" into "b0". */
     for(i = 1; i <= n; i++)
         {
          for(k = 1; k <= n; k++) a0[i][k] = a[i][k];
          b0[i] = b[i];
         }

     ludcmp(a, n, indx, &d);
     lubksb(a, n, indx, b);
     improve_sol(a0, a, n, indx, b0, b);

     for(i = 1; i <= n; i++) free(a0[i]); free(a0); free(b0); free(indx);

     return;
    }


static void lubksb(double **a, unsigned short n, const int *indx, double *b)
   {
    unsigned short i, ii = 0, ip, j;
    double sum;

    for (i = 1; i <= n; i++)
       {
        ip = indx[i];
        sum = b[ip];
        b[ip] = b[i];

        if(ii) for (j = ii; j <= i-1; j++) sum -= a[i][j]*b[j];
        else if(sum) ii = i;

        b[i] = sum;
       }

    for(i = n; i >= 1; i--)
       {
        sum = b[i];

        for(j = i+1; j <= n; j++) sum -= a[i][j]*b[j];
        b[i] = sum/a[i][i];
       }
    return;
   }


#define TINY 1.0E-20

static void ludcmp(double **a, unsigned short n, int *indx, double *d)
   {
    unsigned short i, imax, j, k;
    double big, dum, sum, temp;
    double *vv;
    FILE *lptr = stdout;

    vv = (double *)malloc(sizeof(double)*(n+1));

    *d = 1.0;

   for(i = 1; i <= n; i++)
      {
       big = 0.0;

    // Checking for singularity.
       for (j = 1; j <= n; j++) if( (temp = fabs(a[i][j])) > big) big = temp;
       if(big == 0.0)
           {
            fprintf(lptr, "ERROR: In 'ludcmp()': Matrix supplied is 'singular'. Exiting..\n");
            free(vv);
            exit(-1);
           }

       vv[i] = 1.0/big; // Scaling.
      }

   for(j = 1; j <= n; j++)
      {
       for (i=1;i<j;i++)
         {
          sum = a[i][j];
          for(k = 1; k < i; k++) sum -= a[i][k]*a[k][j];
          a[i][j] = sum;
         }

      big = 0.0;
      for(i = j; i <= n; i++)
         {
          sum = a[i][j];
          for(k = 1; k < j; k++) sum -= a[i][k]*a[k][j];
          a[i][j] = sum;

          if( (dum = vv[i]*fabs(sum)) >= big)
             {
              big = dum;
              imax = i;
             }
         }

      if(j != imax)
          {
           for(k = 1; k <= n; k++)
              {
               dum = a[imax][k];
               a[imax][k] = a[j][k];
               a[j][k] = dum;
              }

           *d = -(*d);
           vv[imax] = vv[j];
          }

      indx[j] = imax;

      if(a[j][j] == 0.0) a[j][j] = TINY;

      if(j != n)
           {
            dum = 1.0/(a[j][j]);
            for(i = j+1; i <= n; i++) a[i][j] *= dum;
           }
     }

   free(vv);

   return;
  }

static void improve_sol(double **a0, double **alud, unsigned short n, const int *indx, const double *b0, double *x)
  {
   unsigned short i, j;
   double sdp;
   double *r;

   r = (double *)malloc(sizeof(double)*(n+1));

   for(i = 1; i <= n; i++)
       {
        sdp = -b0[i];
        for(j = 1; j <= n; j++) sdp += a0[i][j]*x[j];
        r[i] = sdp;
       }

   lubksb(alud, n, indx, r);

   for(i = 1; i <= n; i++) x[i] -= r[i];

   free(r);
  }
